#!/usr/bin/env python
import rospy
from geometry_msgs.msg import Twist
from turtlesim.srv import *
from beginner_tutorials.srv import AddTwoInts,AddTwoIntsResponse


def turtle_ctrl():
    # Starts a new node
    rospy.init_node('robot_cleaner', anonymous=True)
    velocity_publisher = rospy.Publisher('/turtle1/cmd_vel', Twist, queue_size=10)
    vel_msg = Twist()
    PI = 3.1415926535897

    def turtle_move():
        #Receiveing the user's input
        print("Let's move your robot")
        speed = 1
        distance = 3
        isForward = True

        #Checking if the movement is forward or backwards
        if(isForward):
	    vel_msg.linear.x = abs(speed)
        else:
	    vel_msg.linear.x = -abs(speed)
        #Since we are moving just in x-axis
        vel_msg.linear.y = 0
        vel_msg.linear.z = 0
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0
        vel_msg.angular.z = 0
	    #Setting the current time for distance calculus
        t0 = rospy.Time.now().to_sec()
        current_distance = 0

	    #Loop to move the turtle in an specified distance
        while (not rospy.is_shutdown()) and (current_distance < distance):
	        #Publish the velocity
            velocity_publisher.publish(vel_msg)
	        #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()
	        #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
	    #After the loop, stops the robot
        vel_msg.linear.x = 0
	    #Force the robot to stop
        velocity_publisher.publish(vel_msg)


    def turtle_rotate():
        # Receiveing the user's input
        print("Let's rotate your robot")
        speed = 15
        angle = 90
        clockwise = 0

        #Converting from angles to radians
        angular_speed = speed*2*PI/360
        relative_angle = angle*2*PI/360

        #We wont use linear components
        vel_msg.linear.x=0
        vel_msg.linear.y=0
        vel_msg.linear.z=0
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0

        # Checking if our movement is CW or CCW
        if clockwise:
            vel_msg.angular.z = -abs(angular_speed)
        else:
            vel_msg.angular.z = abs(angular_speed)
        # Setting the current time for distance calculus
        t0 = rospy.Time.now().to_sec()
        current_angle = 0

        while(current_angle < relative_angle):
            velocity_publisher.publish(vel_msg)
            t1 = rospy.Time.now().to_sec()
            current_angle = angular_speed*(t1-t0)


        #Forcing our robot to stop
        vel_msg.angular.z = 0
        velocity_publisher.publish(vel_msg)
        #rospy.spin()

    def turtle_circle():
        #Receiveing the user's input
        print("Let's move your robot")
        speed = 1
        distance = 2*PI*speed
        isForward = True

        #Checking if the movement is forward or backwards
        if(isForward):
	    vel_msg.linear.x = abs(speed)
        else:
	    vel_msg.linear.x = -abs(speed)
        #Since we are moving just in x-axis
        vel_msg.linear.y = 0
        vel_msg.linear.z = 0
        vel_msg.angular.x = 0
        vel_msg.angular.y = 0
        vel_msg.angular.z = 1.5
	    #Setting the current time for distance calculus
        t0 = rospy.Time.now().to_sec()
        current_distance = 0

	    #Loop to move the turtle in an specified distance
        while (not rospy.is_shutdown()) and (current_distance < distance):
	        #Publish the velocity
            velocity_publisher.publish(vel_msg)
	        #Takes actual time to velocity calculus
            t1=rospy.Time.now().to_sec()
	        #Calculates distancePoseStamped
            current_distance= speed*(t1-t0)
	    #After the loop, stops the robot
        #vel_msg.linear.x = 0
        #vel_msg.linear.z = 0
	    #Force the robot to stop
        #velocity_publisher.publish(vel_msg)
  
    turtle_move()
    turtle_rotate()
    turtle_move()
    turtle_rotate()
    turtle_move()
    turtle_rotate()
    turtle_move()
    turtle_rotate()
    turtle_circle()

def handle_add_two_ints(req):
    print("Returning [%s + %s = %s]"%(req.a, req.b, (req.a + req.b)))
    return AddTwoIntsResponse(req.a + req.b)

def add_two_ints_server():
    #rospy.init_node('add_two_ints_server')
    s = rospy.Service('add_two_ints', AddTwoInts, handle_add_two_ints)
    print("Ready to add two ints.")
    rospy.spin()

def add_two_ints_client(x, y):
    rospy.wait_for_service('add_two_ints')
    try:
        add_two_ints = rospy.ServiceProxy('add_two_ints', AddTwoInts)
        resp1 = add_two_ints(x, y)
        return resp1.sum
    except rospy.ServiceException as e:
        print("Service call failed: %s"%e)


if __name__ == '__main__':
    try:
        #Testing our function
        turtle_ctrl()
        #add_two_ints_server()
        add_two_ints_client(1,1)
    except rospy.ROSInterruptException: pass
